/* Ultimate Arcade PCB
 * Copyright (C) 2011 Alan Chatham <alan@openchord.org>
 *
 * Based on works by:
 *   Josh Kropf <http://git.slashdev.ca/ps3-teensy-hid>
 *   grunskis <http://github.com/grunskis/gamepad>
 *   Toodles <http://forums.shoryuken.com/showthread.php?t=131230>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

/* Keyboard example for Teensy USB Development Board
 * http://www.pjrc.com/teensy/usb_keyboard.html
 * Copyright (c) 2008 PJRC.COM, LLC
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */


#include <avr/io.h>
#include <util/delay.h>
#include <avr/wdt.h>
#include <avr/eeprom.h>
#include "dataForController_t.h"
#include "physicalButtonList_t.h"
#include "usb_gamepad.h"
#include "PS2/PS2interface.h"
#include "Wii/wiiInterface.h"


#define CPU_PRESCALE(n)	(CLKPR = 0x80, CLKPR = (n))
// Divide this by 10 to get the number of ms to wait while the programming
//  button is down until canceling out of programming mode
#define PROGRAMMING_TIMEOUT 20000


// buttonList is a struct that holds the information about what
//  actual buttons are being pressed on the controller.
//  It gets updated by ReadButtons()
physicalButtonList_t buttonList;

// buttonArray is an array of uint8_t's representing
//  an ordered list of the buttons physically pressed
//  on the controller.  The order isn't important, so
//  long as it doesn't change.  This gets filled by readButtons()
uint8_t* buttonArray[NUMBER_OF_INPUTS];
// buttonMapArray is an array that maps inputs to outputs.
//  Each input is represented by an element in the array, and the
//  value at that array index is what logical button it outputs
uint8_t EEMEM buttonMapArray[NUMBER_OF_INPUTS];
// buttonLockingArray is an array that is used to apply a locking
//  feature to specific buttons.  If buttonLockingArray[buttonNumber]
//  is in any of the lockStates other than cantLock, pressing and releasing
//  that button will cycle it through the lockStates
uint8_t buttonLockingArray[NUMBER_OF_INPUTS];
uint8_t EEMEM buttonLockingStorage[NUMBER_OF_INPUTS];
enum lockState{cantLock, unlocked, lockingPress, locked, unlockingPress};
	
// rapdiFireArray is an array that holds values for how fast buttons get pressed.
//  Rapid fire works by decrementing the values in the buttons every main loop
//  and 'pressing' the button only when the value reaches a certain cutoff, and 
//  resetting when the value reaches zero.
uint16_t rapidFireArray[NUMBER_OF_INPUTS];
uint16_t EEMEM rapidFireResetArray[NUMBER_OF_INPUTS]; /* = {10, 20, 30, 40, 50, 60,
											 70, 80, 90, 100, 110, 120,
											 130, 140, 1500, 1600, 1700, 1800,
											 1900, 2000, 2100, 2200, 2300, 2400,
											 2500, 2600, 2700, 2800, 2900, 3000,
											 3100, 3200, 3300, 3400, 3500, 3600};*/
#define RAPID_FIRE_PRESS_CUTOFF 5
#define RAPID_FIRE_MIN 0
#define RAPID_FIRE_MAX 130


// This function initializes the button setup, turning on pull-up resistors
//  for buttons and outputting the proper power levels on the Joy+ and Joy- pins
void initPins(void){
	#ifdef December2011Board
	
		// Input, Pull-up resistors on for buttons:
		//  R1, R2, R3, Start, Circle, Triangle, Cross, Square
		DDRA = 0;
		PORTA = 0xFF;
		
		// Input, Pull-up resistors on for: Extra D1, D2, D3
		// Other pins are all used for PS2 communication
		DDRB  &= 0b00011111;
		PORTB |= 0b11100000;
		
		// Input, Pull-up resistors on for: L2, Select, L3, _, _, DDown, DRight, DLeft 
		// Output: LOW Joy-, HIGH - Joy+
		DDRC  = 0b00011000;
		PORTC = 0b11110111;
		
		// Output: LOW Common2, _, LOW LED
		// Input: _, Home, _, Program, HF1, HF2
		//  Last 2 are Wii SCL and SDA
		DDRD  = 0b10100000;
		PORTD = 0b01011100;
		
		// Input, Pull-up resistors on for: D6, D5, HF3, HF4, D4, L1, DUp, _
		// Output: LOW Common1
		DDRE  = 0b00000001;
		PORTE = 0b11111110;
		
		// PORTF is all analog
	
	#endif
}


// Read digital pin macro.  Returns 1 if the pin is LOW, 0 if the pin is HIGH
//  This is because all our digital pins are pulled up high
#define readPin(pin, pinNumber) (!( (pin) & (1 << pinNumber) ))

//Set up the ADC to read analog data on the analog ports
void setupADC(void)
{
	// First, turn off the digital I/O on the ADC pins to save power
	DIDR0 = 0xFF;
	// Set ADC prescaler to 128 - 125KHz sample rate @ 16MHz 
    ADCSRA |= (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); 
    ADMUX |= (1 << REFS0); // Set ADC reference to AVCC 
    ADMUX |= (1 << ADLAR); // Left adjust ADC result to allow easy 8 bit reading 
    // No MUX values needed to be changed to use ADC0 
	ADMUX &= ~0b1111;
	// Finally, enable the ADC
	ADCSRA |= (1 << ADEN);
}

// Read analog pin code
uint8_t readAnalog(uint8_t pinNumber){
	// First, set the ADC to the proper pin
	ADMUX = (ADMUX & 0b11110000) | pinNumber;
	// Enable the ADC
	ADCSRA |= (1 << ADSC);
	// Loop until we have a reading
	while (ADCSRA & (1 << ADSC));
	return ADCH;	
}



void readButtons(void){
	
	buttonList.r1On = readPin(R1_PIN, R1);
	buttonList.r2On = readPin(R2_PIN, R2);
	buttonList.r3On = readPin(R3_PIN, R3);
	buttonList.startOn = readPin(START_PIN, START);
	buttonList.circleOn = readPin(CIRCLE_PIN, CIRCLE);
	buttonList.triangleOn = readPin(TRIANGLE_PIN, TRIANGLE);
	buttonList.squareOn = readPin(SQUARE_PIN, SQUARE);
	buttonList.crossOn = readPin(CROSS_PIN, CROSS);
	buttonList.l1On = readPin(L1_PIN, L1);
	buttonList.l2On = readPin(L2_PIN, L2);
	buttonList.l3On = readPin(L3_PIN, L3);
	buttonList.menuOn = readPin(MENU_PIN, MENU);
	buttonList.selectOn = readPin(SELECT_PIN, SELECT);
	
	buttonList.dpadDownOn = readPin(DPADDOWN_PIN, DPADDOWN);
	buttonList.dpadRightOn = readPin(DPADRIGHT_PIN, DPADRIGHT);
	buttonList.dpadLeftOn = readPin(DPADLEFT_PIN, DPADLEFT);
	buttonList.dpadUpOn = readPin(DPADUP_PIN, DPADUP);
	
	buttonList.joy3ButtonOn = readPin(JOY3_BUTTON_PIN, JOY3_BUTTON);
	buttonList.joy4ButtonOn = readPin(JOY4_BUTTON_PIN, JOY4_BUTTON);
	buttonList.handsFree1 = readPin(HANDSFREE_1_PIN, HANDSFREE_1);
	buttonList.handsFree2 = readPin(HANDSFREE_2_PIN, HANDSFREE_2);
	buttonList.handsFree3 = readPin(HANDSFREE_3_PIN, HANDSFREE_3);
	buttonList.handsFree4 = readPin(HANDSFREE_4_PIN, HANDSFREE_4);
	buttonList.buttonBelowLStick = readPin(BUTTON_BELOW_L_STICK_PIN, BUTTON_BELOW_L_STICK);
	buttonList.buttonBelowRStick = readPin(BUTTON_BELOW_R_STICK_PIN, BUTTON_BELOW_R_STICK);
	buttonList.sipSwitch = readPin(SIP_SWITCH_PIN, SIP_SWITCH);
	buttonList.puffSwitch = readPin(PUFF_SWITCH_PIN, PUFF_SWITCH);
	
	buttonList.leftStickX = readAnalog(LEFTSTICKX);
	buttonList.leftStickY = readAnalog(LEFTSTICKY);
	buttonList.rightStickX = readAnalog(RIGHTSTICKX);
	buttonList.rightStickY= readAnalog(RIGHTSTICKY);
	buttonList.joy3X = readAnalog(JOY3X);
	buttonList.joy3Y = readAnalog(JOY3Y);
	buttonList.joy4X = readAnalog(JOY4X);
	buttonList.joy4Y = readAnalog(JOY4Y);
	
	buttonList.programButtonOn = readPin(PROGRAM_PIN,PROGRAM);
}

// This function uses the data from the button presses
//  to cycle through the locking state of the buttons
void setLockingArray(void){
	for (uint8_t i = 0; i < NUMBER_OF_INPUTS; i++){
		// If the button is pressed
		if (*buttonArray[i] == 1){
			if (buttonLockingArray[i] == unlocked)
				buttonLockingArray[i] = lockingPress;
			if (buttonLockingArray[i] == locked)
				buttonLockingArray[i] = unlockingPress;
		}
		// If the button is unpressed
		else{
			if (buttonLockingArray[i] == lockingPress)
				buttonLockingArray[i] = locked;
			if (buttonLockingArray[i] == unlockingPress)
				buttonLockingArray[i] = unlocked;
		}
	}
}	

// This function cycles through all the physical buttons and uses
//  that data to apply a rapid-fire functionality to them by
//  selectively 'turning off' button presses
void applyRapidFire(void){
	for (uint8_t i = 0; i < NUMBER_OF_INPUTS; i++){
		// If the button isn't pressed, set it up to press next time
		if (*buttonArray[i] == 0)
			rapidFireArray[i] = RAPID_FIRE_PRESS_CUTOFF;
		if ((*buttonArray[i] == 1) && (rapidFireArray[i] > 0)){
			rapidFireArray[i]--;
			// Turn off the button some of the time
			if (rapidFireArray[i] > RAPID_FIRE_PRESS_CUTOFF)
				*buttonArray[i] = 0;
		}
		// If rapidFireArray is at zero for that button,
		//  reset the rapid fire timer for that button
		else if (rapidFireArray[i] == 0) {
			rapidFireArray[i] = eeprom_read_word(&rapidFireResetArray[i]);
		}
	
	}	
}

// This function applies a map to the data, turning the data from
//  the physical buttons to virtual buttons
void transformButtonsPressed(void){
	// This stores a temporary copy of the 
	uint8_t newButtonArray[NUMBER_OF_INPUTS];
	// Fill it with zeroes to give us a blank slate
	for (uint8_t i = 0; i < NUMBER_OF_INPUTS; i++){
			newButtonArray[i] = 0;
	}
	
	// Use the actual button press data to control the 
	//  locking behavior
	setLockingArray();	
	// Now we can trash the actual button data with our
	//  locking button data
	for (uint8_t i = 0; i < NUMBER_OF_INPUTS; i++){
		if (buttonLockingArray[i] == locked)
			*buttonArray[i] = 1;
	}

	
	// We've taken care of locking. Now for rapid fire.
	applyRapidFire();
	// Next, take the physical button presses, convert them
	//  through the mapping function, and write the new
	//  button press data with the mapped button data
	//  Note: buttonArray is an array of pointers to the actual press data
	for (uint8_t i = 0; i < NUMBER_OF_INPUTS; i++){
		newButtonArray[eeprom_read_byte(&buttonMapArray[i])] |= *buttonArray[i];
	}
	// Finally, we copy the new button mapping back into
	//  the buttonArray, replacing the physical button presses
	//  with the abstracted ones
	for (uint8_t i = 0; i < NUMBER_OF_INPUTS; i++){
		*buttonArray[i] = newButtonArray[i];
	}
}

// Reads the buttons and returns the number of the first button it
//  finds that is pressed down.
//  Returns -1 if no button is pressed
int8_t readOneButtonPress(void){
	readButtons();
	for (int8_t i = 0; i < NUMBER_OF_DIGITAL_BUTTONS; i++){
		if (*buttonArray[i] == 1)
			return i;
	}
	// If we're here, no button was pressed, so let the caller know that
	return -1;
}

// This sets up an empty controller data packet and sends it out
//  to all the controllers attached.
void setControllersToZero(void){
	dataForController_t emptyData;
	usb_gamepad_reset_state();
	usb_gamepad_send();
}
// This function is entered when we press the program button, and holds
//  the program focus until programming is completed
void programmingMode(void){
	// First, let's clear the controller data and 
	//  send that data out to make sure we're not holding some random button
	setControllersToZero();
	
	// Turn on the LED to indicate programming mode
	LED_PORT |= (1 << LED);
	
	uint16_t timer = 0;
	// Count how long we're holding down our programming button
	while( readPin(PROGRAM_PIN, PROGRAM) && timer < PROGRAMMING_TIMEOUT){
		timer++;
		_delay_ms(100);
	}
	// Debounce or cancel out if we're not doing anything too long
	if (timer < 2 || timer > PROGRAMMING_TIMEOUT)
		return;

	// Read in a button to find out what to remap
	// Wait for a button to be pressed
	timer = 0;
	int8_t buttonToMapTo = -1;
	while(timer < PROGRAMMING_TIMEOUT && buttonToMapTo == -1){
		timer++;
		_delay_ms(100);
		buttonToMapTo = readOneButtonPress();
	}
	LED_PORT &= ~(1 << LED); // Turn off the LED
	// Then wait for it to be released
	timer = 0;
	int8_t buttonHeldDown = buttonToMapTo;
	while(timer < PROGRAMMING_TIMEOUT && buttonHeldDown != -1){
		_delay_ms(100);
		buttonHeldDown = readOneButtonPress();
	}
	LED_PORT |= (1 << LED); // Turn the LED back on
	if (buttonToMapTo == -1) // Timed out
		return;
	
	// Debouncing delay so we don't instantly remap to the same button
	_delay_ms(100);
	
	// Read in another button to find out what to remap it to

	int8_t buttonToReprogram = -1;
	while(buttonToReprogram == -1){
		_delay_ms(100);
		buttonToReprogram = readOneButtonPress();
	}
	// Turn the LED off while a button is held
	LED_PORT &= ~(1 << LED);
	// If we keep holding that button down, we'll activate locking
	//  or rapid fire
	buttonHeldDown = buttonToReprogram;
	int8_t lockingFlag = cantLock;
	uint8_t rapidFireRate = 0;
	while(buttonHeldDown != -1){
		timer++;
		_delay_ms(10);
		buttonHeldDown = readOneButtonPress();
		// If we've held for 5 seconds, set this as a locking button
		//  and flash the LED twice
		if (timer == 500){
			lockingFlag = unlocked;
			LED_PORT |= (1 << LED);
			_delay_ms(250);
			LED_PORT &= ~(1 << LED);
			_delay_ms(250);
			LED_PORT |= (1 << LED);
			_delay_ms(250);
			LED_PORT &= ~(1 << LED);
		}
		if (timer >= 900){ // Actually 10 seconds, since we take 1 second to flash
			lockingFlag = 0;
			// Get a good value for the rapid fire from the left analog stick
			rapidFireRate = 128 - (readAnalog(LEFTSTICKY) / 2);
			if (rapidFireRate < RAPID_FIRE_MIN)
				rapidFireRate = RAPID_FIRE_MIN;
			// And flash the programming LED at the proper rate
			if ((timer % rapidFireRate) > RAPID_FIRE_PRESS_CUTOFF)
				LED_PORT &= ~(1 << LED);
			else
				LED_PORT |= (1 << LED);
		}	
	}
	
	// Actually set locking and rapid fire
	eeprom_write_byte(&buttonLockingStorage[buttonToReprogram], lockingFlag);// = lockingFlag;
	buttonLockingArray[buttonToReprogram] = lockingFlag;
	eeprom_write_word(&rapidFireResetArray[buttonToReprogram], rapidFireRate);// = rapidFireRate;

	// If we're down here, we got good data and we can complete
	//  the button remapping process.
	if (buttonToReprogram >= 0 && buttonToReprogram < NUMBER_OF_INPUTS)
		eeprom_write_byte(&buttonMapArray[buttonToReprogram], buttonToMapTo);// = buttonToMapTo;
		
	// And write all the new data to the EEPROM
		
	// And turn off the LED to give confirmation that the change happened
	LED_PORT &= ~(1 << LED);
	return;
}	

dataForController_t buttonListToDataForController(physicalButtonList_t btnList){
	dataForController_t dataToSend;
	dataToSend.circleOn = btnList.circleOn;
	dataToSend.crossOn  = btnList.crossOn;
	dataToSend.dpadDownOn = btnList.dpadDownOn;
	dataToSend.dpadLeftOn = btnList.dpadLeftOn;
	dataToSend.dpadRightOn = btnList.dpadRightOn;
	dataToSend.dpadUpOn = btnList.dpadUpOn;
	dataToSend.homeOn = btnList.menuOn;
	dataToSend.l1On = btnList.l1On;
	dataToSend.l2On = btnList.l2On;
	dataToSend.l3On = btnList.l3On;
	dataToSend.leftStickX = btnList.leftStickX;
	dataToSend.leftStickY = btnList.leftStickY;
	dataToSend.r1On = btnList.r1On;
	dataToSend.r2On = btnList.r2On;
	dataToSend.r3On = btnList.r3On;
	dataToSend.rightStickX = btnList.rightStickX;
	dataToSend.rightStickY = btnList.rightStickY;
	dataToSend.selectOn = btnList.selectOn;
	dataToSend.squareOn = btnList.squareOn;
	dataToSend.startOn = btnList.startOn;
	dataToSend.triangleOn = btnList.triangleOn;
	return dataToSend;
}

//! Declare function pointer to USB bootloader entry point 
void (*start_bootloader) (void)=(void (*)(void))0xf000; 

int main(void) {
	// First things first.  If we're holding certain buttons on plug-in,
	// then reset
	initPins();
	setupADC();
	readButtons();
	if(buttonList.l1On == 1){
		start_bootloader();
	}
	wdt_disable();
	// set for 16 MHz clock
	CPU_PRESCALE(0);

	initPins();
	setupADC();
	startPS2Communication();

	// Initialize the USB, and then wait for the host to set configuration.
	// If the Teensy is powered without a PC connected to the USB port,
	// this will wait forever.
	usb_init();
//	while (!usb_configured()); // wait

	// Wait an extra second for the PC's operating system to load drivers
	// and do whatever it does to actually be ready for input
//	_delay_ms(500);
	
	buttonArray[0] = &buttonList.r1On;
	buttonArray[1] = &buttonList.r2On;
	buttonArray[2] = &buttonList.r3On;
	buttonArray[3] = &buttonList.startOn;
	buttonArray[4] = &buttonList.circleOn;
	buttonArray[5] = &buttonList.triangleOn;
	buttonArray[6] = &buttonList.squareOn;
	buttonArray[7] = &buttonList.crossOn;
	buttonArray[8] = &buttonList.l1On;
	buttonArray[9] = &buttonList.l2On;
	buttonArray[10] = &buttonList.l3On;
	buttonArray[11] = &buttonList.menuOn;
	buttonArray[12] = &buttonList.selectOn;
	
	buttonArray[13] = &buttonList.dpadDownOn;
	buttonArray[14] = &buttonList.dpadRightOn;
	buttonArray[15] = &buttonList.dpadLeftOn;
	buttonArray[16] = &buttonList.dpadUpOn;
	
	buttonArray[17] = &buttonList.joy3ButtonOn;
	buttonArray[18] = &buttonList.joy4ButtonOn;
	buttonArray[19] = &buttonList.handsFree1;
	buttonArray[20] = &buttonList.handsFree2;
	buttonArray[21] = &buttonList.handsFree3;
	buttonArray[22] = &buttonList.handsFree4;
	buttonArray[23] = &buttonList.buttonBelowLStick;
	buttonArray[24] = &buttonList.buttonBelowRStick;
	buttonArray[25] = &buttonList.sipSwitch;
	buttonArray[26] = &buttonList.puffSwitch;
	
	buttonArray[27] = &buttonList.leftStickX;
	buttonArray[28] = &buttonList.leftStickY;
	buttonArray[29] = &buttonList.rightStickX;
	buttonArray[30] = &buttonList.rightStickY;
	buttonArray[31] = &buttonList.joy3X;
	buttonArray[32] = &buttonList.joy3Y;
	buttonArray[33] = &buttonList.joy4X;
	buttonArray[34] = &buttonList.joy4Y;
	
	buttonArray[35] = &buttonList.programButtonOn;

	// Check to make sure our stored EEPROM variables make sense
	for (uint8_t i = 0; i < NUMBER_OF_INPUTS; i++){
		// First, check the button mapping array
		if (eeprom_read_byte(&buttonMapArray[i]) >= NUMBER_OF_INPUTS)
			eeprom_write_byte(&buttonMapArray[i], i);// = i;
			
		// Next, check the button locking data, and load in the stored locking data
		if (eeprom_read_byte(&buttonLockingStorage[i]) >= 5)
			eeprom_write_byte(&buttonLockingStorage[i], cantLock);
		else
			buttonLockingArray[i] = eeprom_read_byte(&buttonLockingStorage[i]);
		
		// Finally, check the rapid fire data
		if (eeprom_read_word(&rapidFireResetArray[i]) > RAPID_FIRE_MAX)
			eeprom_write_word(&rapidFireResetArray[i], RAPID_FIRE_MIN);
	}
	
	
	startWiiCommunication();
	
	while (1) {
		// First, reset the internal state of the gamepad
		usb_gamepad_reset_state();

		// Next, we need to read the physical buttons and store them into an abstract
		//  representation of what physical buttons are being held
		readButtons();
		
		// Check if we're trying to enter programming mode,
		//  if so, do the programming routine and make sure the LED
		//  gets turned off at the end.
		if (readPin(PROGRAM_PIN,PROGRAM)){
			programmingMode();
			LED_PORT &= ~(1 << LED);
		}
		
		// After we know what physical buttons are being held, we can map those buttons
		//  to the output buttons we want to send
		transformButtonsPressed();
		
	//	gamepad_state.start_btn = buttonsPressed.startOn;
	//	gamepad_state.triangle_btn = buttonList.triangleOn;//Button 4
	//	gamepad_state.square_btn = *buttonArray[6];
	//	gamepad_state.circle_btn = buttonsPressed.circleOn;
	//	gamepad_state.cross_btn = buttonsPressed.crossOn;
		// Finally, send the data
		dataForController_t dataToSend = buttonListToDataForController(buttonList);
		sendPS3Data(dataToSend);
		sendPS2Data(dataToSend);
		sendWiiData(dataToSend);
		
		//usb_gamepad_send();
	}
}

